import redis
import json
import re
import pickle


r = redis.Redis(db=0)

gps_point = {
    'C2': [30.8760628, 121.2468456],
    'C1': [30.8757288, 121.2469029],
    'B2': [30.8760882, 121.2468537],
    'B1': [30.8757437, 121.2469275]
}

greenhouse_points_backup = {
    'A1': [-0.01, 0.00, 0.00],
    'A2': [0.13, 1.58, 6.02],
    'A3': [0.39, 4.16, 15.70],
    'A4': [0.63, 6.84, 25.80],
    'A5': [0.98, 10.06, 38.07],
    'A6': [1.09, 11.04, 41.93],

    'B1':[0.00, 0.00, 0.00],
    'B2': [0.19, 1.50, 5.35],
    'B3': [0.52, 4.24, 14.95],
    'B4': [0.84, 7.18, 25.22],
    'B5': [1.14, 9.53, 33.52],
    'B6': [1.31, 10.83, 38.15],

    'C6': [0.58, 9.22, 34.53],
    'C5': [0.46, 6.97, 25.94],
    'C4': [0.24, 4.23, 15.72],
    'C3': [0.15, 2.42, 9.07],
    'C2': [0.08, 1.12, 4.25],
    'C1': [0.05, -0.05, -0.18],

    'D1': [0.00,0.00, 0.00],
    'D2': [0.00, 1.40, 5.19],
    'D3': [-0.02, 2.43, 8.93],
    'D4': [-0.00, 4.26, 15.66],
    'D5': [0.00, 6.14, 22.55],
    'D6': [-0.01, 8.53, 31.43],

    'E1': [0.00, 0.00, 0.00],
    'E2': [0.05, 1.38, 4.66],
    'E3': [0.14, 3.64, 12.17],
    'E4': [0.31, 6.25, 21.03],
    'E5': [0.45, 7.84, 26.58],
    'E6': [0.53, 9.08, 30.70]


}

greenhouse_points = {
    'A1': [-0.05, -0.18, -0.70],
    'A2': [-0.069, 0.55, 1.74],
    'A3': [-0.07, 3.91, 12.06],
    'A4': [-0.14, 6.59, 20.35],
    'A5': [-0.23, 9.89, 30.69],
    'A6': [-0.31, 10.88, 33.42],

    'B1': [-0.00, -0.02, -0.06],
    'B2': [0.04, 1.02, 3.42],
    'B3': [0.20, 3.31, 11.52],
    'B4': [0.33, 5.89, 20.41],
    'B5': [0.46, 8.77, 30.57],
    'B6': [0.42, 9.67, 33.33],

    'C1': [0.01, -0.03, -0.07],
    'C2': [0.03, 0.54, 2.05],
    'C3': [0.24, 3.11, 11.16],
    'C4': [0.45, 5.49, 19.92],
    'C5': [0.72, 8.33, 30.37],
    'C6': [0.79, 9.21, 33.39]
}

with open('waypoints.json', 'r') as fp:
    gps_location_list = json.load(fp)

# with open('gps_location.pickle', 'rb') as f:
#     gps_location_list = pickle.load(f)

def atoi(text):
    return int(text) if text.isdigit() else text


def natural_keys(text):
    return [atoi(c) for c in re.split(r'(\d+)', text)]

def text_processing(keys):
    if ',' in keys:
        points = keys.split(',')
        if len(points) == 3:
            r.rpush('job_queue', json.dumps(['line', [gps_location_list[points[0]]['cor'], gps_location_list[points[1]]['cor'], int(points[2])]]))
        else:
            r.rpush('job_queue', json.dumps(['line', [gps_location_list[points[0]]['cor'], gps_location_list[points[1]]['cor'], 0]]))

    elif keys == 'clr':
        r.delete('job_queue')
    elif keys == 'pop':
        r.lpop('job_queue')
    elif keys.startswith('chain'):
        lane_name = keys.split(' ')[1]
        points_list = [x for x in list(gps_location_list.keys()) if x.startswith(lane_name)]

        points_list.sort(key=natural_keys)

        if len(keys.split(' ')) == 3:
            offset = int(keys.split(' ')[2])
        else:
            offset = 0

        index = 0

        while index < len(points_list) - 1:
            r.rpush('job_queue', json.dumps(['line', [points_list[index], points_list[index + 1], offset]]))
            index = index + 1
    elif keys.startswith('reverse_chain'):
        lane_name = keys.split(' ')[1]
        points_list = [x for x in list(gps_location_list.keys()) if x.startswith(lane_name)]

        points_list.sort(key=natural_keys, reverse=True)

        if len(keys.split(' ')) == 3:
            offset = int(keys.split(' ')[2])
        else:
            offset = 0

        index = 0

        while index < len(points_list) - 1:
            r.rpush('job_queue', json.dumps(['line', [points_list[index], points_list[index + 1], offset]]))
            index = index + 1
    elif keys.startswith('op'):
        code = keys.split(' ')[1]
        time_to_sleep = keys.split(' ')[2]
        r.rpush('job_queue', json.dumps(['operation', [code, time_to_sleep]]))
    elif keys.startswith('thread_op'):
        code = keys.split(' ')[1]
        time_to_sleep = keys.split(' ')[2]
        r.rpush('job_queue', json.dumps(['operation_thread', [code, time_to_sleep]]))
    elif keys == 'left_u':
        r.rpush('job_queue', json.dumps(['left_u_turn', []]))
    elif keys == 'right_u':
        r.rpush('job_queue', json.dumps(['right_u_turn', []]))
    elif keys == 'left':
        r.rpush('job_queue', json.dumps(['left', []]))
    elif keys == 'right':
        r.rpush('job_queue', json.dumps(['right', []]))
    elif keys == 'left_adjust':
        r.rpush('job_queue', json.dumps(['left_adjust', []]))
    elif keys == 'right_adjust':
        r.rpush('job_queue', json.dumps(['right_adjust', []]))
    elif keys.startswith('search'):
        search_item = keys.split(' ')[1]
        print([x for x in list(gps_location_list.keys()) if search_item in x])
    elif keys.startswith('vslam_reverse'):
        startpoint = keys.split(' ')[1]
        endpoint = keys.split(' ')[2]

        if len(keys.split(' ')) == 4:
            no_stop = 1
        else:
            no_stop = 0

        r.rpush('job_queue', json.dumps(['reverse_line', [greenhouse_points[startpoint], greenhouse_points[endpoint], no_stop]]))

    elif keys.startswith('vslam_line'):
        startpoint = keys.split(' ')[1]
        endpoint = keys.split(' ')[2]

        if len(keys.split(' ')) == 4:
            no_stop = 1
        else:
            no_stop = 0

        r.rpush('job_queue', json.dumps(['openvslam_line', [greenhouse_points[startpoint], greenhouse_points[endpoint], no_stop]]))
    elif keys.startswith('fix_speed'):
        left_speed = keys.split(' ')[1]
        right_speed = keys.split(' ')[2]
        distance_lock = keys.split(' ')[3]
        r.rpush('job_queue', json.dumps(['fix_speed', [int(left_speed), int(right_speed), float(distance_lock)]]))
    elif keys.startswith('map_open'):
        map_file = keys.split(' ')[1]
        r.rpush('job_queue', json.dumps(['map_open', [map_file]]))
    elif keys.startswith('map_close'):
        r.rpush('job_queue', json.dumps(['map_close', []]))
    elif keys.startswith('roll_down'):
        r.rpush('job_queue', json.dumps(['roll_down', []]))
    elif keys.startswith('roll_up'):
        r.rpush('job_queue', json.dumps(['roll_up', []]))
    elif keys.startswith('set_th'):
        threshold = keys.split(' ')[1]
        r.rpush('job_queue', json.dumps(['set_th', [int(threshold)]]))
    elif keys.startswith('gps_line'):
        startpoint = keys.split(' ')[1]
        endpoint = keys.split(' ')[2]
        r.rpush('job_queue', json.dumps(['gps_line', [gps_point[startpoint], gps_point[endpoint]]]))

second_lane = [
    'chain B6_L2 -50', 'right', 'right_adjust', 'B6_C2_2,B5_C2_1', 'right',
    'op DOWN','reverse_chain B5_L2 +50', 'op UP', 'left', 'left_adjust', 'B5_C1_2,B4_C1_1', 'left',
    'op DOWN', 'chain B4_L2 -50', 'op UP', 'right', 'right_adjust', 'B4_C2_2,B3_C2_1', 'right',
    'op DOWN', 'reverse_chain B3_L2 +50', 'op UP', 'left', 'B3_C1_2,B2_C1_1', 'left',
    'op DOWN', 'chain B2_L2 -50', 'op UP', 'right', 'right_adjust', 'B2_C2_2,B1_C2_1', 'right',
    'op DOWN', 'reverse_chain B1_L2 +50', 'op UP', 'left', 'left_adjust', 'B1_C1_2,B10_C1_1', 'left',
    'op DOWN', 'chain B10_L2 -50', 'op UP', 'right', 'right_adjust', 'B10_C2_2,B11_C2_1', 'right',
    'op DOWN', 'reverse_chain B11_L2 +50', 'op UP', 'left', 'left_adjust', 'B11_C1_2,B12_C1_1', 'left',
    'op DOWN', 'chain B12_L2 -50', 'op UP', 'right', 'right_adjust', 'B12_C2_2,B13_C2_1', 'right',
    'reverse_chain B13_L2 +60',

                                'right', 'right_adjust', 'B13_C1_1,B12_C1_3', 'right'
    'thread_op DOWN 15', 'chain B12_L3 +100', 'op UP 10', 'left', 'left_adjust', 'B12_C2_1,B11_C2_3', 'left',
    'thread_op DOWN 15', 'reverse_chain B11_L3 -100', 'op UP 10', 'right', 'right_adjust', 'B11_C1_1,B10_C1_3', 'right',
    'thread_op DOWN 15', 'chain B10_L3 +100', 'op UP 10', 'right', 'right_adjust', 'B10_C2_1,B1_C2_3', 'left'
    'thread_op DOWN 15', 'reverse_chain B1_L3 -100'
             ]

second_go_in = ['R1_1,R1_3', 'left', 'E1_OUT,E1_IN']

right_crawl = ['fix_speed -10000 -1000 3',
               'fix_speed -1000 -10000 3',
               'fix_speed 10000 1000 3',
               'fix_speed 1000 10000 3']



left1_end1 = [
    'fix_speed -3000 3000 34',
    'fix_speed 3000 3000 18',
    'fix_speed -3000 3000 32',
]

left2_end2 = [
    # 'fix_speed 3000 3000 22',
    'fix_speed -3000 3000 34',
    'fix_speed 3000 3000 33',
    'fix_speed -3000 3000 32',
]

lane_1 = [
    'set_th 620',

    # 'map_open map_lane1.msg',
    'vslam_line A1 A2',
    'roll_down',
    'vslam_line A2 A3 1',
    'vslam_line A3 A4 1',
    'vslam_line A4 A5',
    'roll_up',
    'vslam_line A5 A6',
    'map_close',
]

lane_3 = [
    'set_th 620',
    'vslam_line C1 C2',
    'roll_down',
    # 'map_open map_lane3.msg',
    'vslam_line C2 C3 1',
    'vslam_line C3 C4 1',
    'vslam_line C4 C5',
    'roll_up',
    'vslam_line C5 C6'
    'map_close',

]

lane_2 = [
    'set_th 680',
    'roll_down',
    'vslam_line E1 E2 ',
    'map_close',
    'roll_up'
]


if __name__ == '__main__':
    while True:
        keys = input('press enter to continue')

        if keys.startswith('combo'):
            combo_name = keys.split(' ')[1]
            exec("[text_processing(x) for x in %s]" % combo_name)
            # if combo_name == 'second_lane':
            #     [text_processing(x) for x in second_lane]
            # if combo_name == 'left_lane':
            #     [text_processing(x) for x in left_lane]
        else:
            text_processing(keys)

        print(r.lrange('job_queue', -10, r.llen('job_queue')))
        print(r.lrange('job_queue', 0, 10))